// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
// Refer to README.md in this directory.
#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>

// ---------------------------------------------------------------------------
// Tracker. Manage tracks. Create, remove, update. 跟踪器。跟踪管理。创建、删除、更新。
// ---------------------------------------------------------------------------
CTracker::CTracker(const Params &parameters) : params(parameters), NextTrackID(0) {}

CTracker::~CTracker(void) {}

void CTracker::updateParameters(const Params &parameters) { params = parameters; }

void CTracker::Update(const std::vector<Point_t> &detectedCentroid,
                      const std::vector<std::vector<cv::Point> > &contours)
{
  // Each contour has a centroid 每个轮廓线都有一个质心
  assert(detectedCentroid.size() == contours.size());
  // -----------------------------------
  // If there is no tracks yet, then every cv::Point begins its own track.
  // 如果还没有轨道，那么每个cv::Point开始它自己的轨道。
  // -----------------------------------
  if (tracks.empty())
  {
    // If no tracks yet
    for (size_t i = 0; i < detectedCentroid.size(); ++i)
    {
      tracks.push_back(std::unique_ptr<CTrack>(
              new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
    }
  }
  size_t N = tracks.size();
  size_t M = detectedCentroid.size();
  assignments_t assignment;
  if (!tracks.empty())
  {
    // Distance matrix of N-th Track to the M-th detectedCentroid
    distMatrix_t Cost(N * M);
    // calculate distance between the blobs centroids
    for (size_t i = 0; i < tracks.size(); i++)
    {
      for (size_t j = 0; j < detectedCentroid.size(); j++)
      {
        Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
      }
    }
    // -----------------------------------
    // Solving assignment problem (tracks and predictions of Kalman filter)
    // 求解赋值问题(卡尔曼滤波的跟踪与预测)
    // -----------------------------------
    AssignmentProblemSolver APS;
    APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
    // -----------------------------------
    // clean assignment from pairs with large distance 从距离大的配对中清除分配
    // -----------------------------------
    for (size_t i = 0; i < assignment.size(); i++)
    {
      if (assignment[i] != -1)
      {
        if (Cost[i + assignment[i] * N] > params.dist_thresh)
        {
          assignment[i] = -1;
          tracks[i]->skipped_frames = 1;
        }
      }
        // If track have no assigned detect, then increment skipped frames counter.
        // 如果track没有指定检测，则增加跳过帧计数。
      else tracks[i]->skipped_frames++;
    }
    // -----------------------------------
    // If track didn't get detects long time, remove it.
    // 如果跟踪长时间没有被检测到，删除它。
    // -----------------------------------
    for (int i = 0; i < static_cast<int>(tracks.size()); i++)
    {
      if (tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
      {
        tracks.erase(tracks.begin() + i);
        assignment.erase(assignment.begin() + i);
        i--;
      }
    }
  }
  // -----------------------------------
  // Search for unassigned detects and start new tracks for them.
  // -----------------------------------
  for (size_t i = 0; i < detectedCentroid.size(); ++i)
  {
    if (find(assignment.begin(), assignment.end(), i) == assignment.end())
    {
      tracks.push_back(
              std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
    }
  }

  // Update Kalman Filters state
  for (size_t i = 0; i < assignment.size(); i++)
  {
    // If track updated less than one time, than filter state is not correct.
    // If we have assigned detect, then update using its coordinates,
    if (assignment[i] != -1)
    {
      tracks[i]->skipped_frames = 0;
      tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
    }
      // if not continue using predictions
    else tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
  }
}

